#!coding:utf-8
import cv2
import threading
import time
import numpy as np
import base64
from subprocess import PIPE, Popen
# from .image_rector import ImageRector
from .cameramodels import PinholeCameraModel
from camera_utils.camera_ws import WebsocketCamera
from camera_utils import load_camera_info_3, cameraList, bgr_from_jpg, toImage
from sensor_msgs.msg import Image, CompressedImage
try:
    from picamera2 import Picamera2
except Exception as e:
    print(e)
    
class UsbCamera(object):
    """docstring for UsbCamera"""

    def __init__(self, rate=30, callback=None, shm=None):
        super(UsbCamera, self).__init__()
        self.use_picam2 = False
        self.picam_ids = []
        picam = None
        '''
        try:
            picam = Picamera2(0)
            config = picam.create_still_configuration(main={"size": (640, 480), 'format': 'RGB888'})
            picam.configure(config)
            picam.start()
            # time.sleep(1)
            frame = picam.capture_array()
            picam.close()
            picam.stop()
            self.use_picam2 = True
        except Exception as e:
            print(e)
            if picam is not None:
                picam.close()
                picam.stop()
        '''
        cmd = ["/usr/bin/libcamera-hello", "--list-cameras"]
        out, err = Popen(cmd, stdout=PIPE, stderr=PIPE).communicate()
        out = out.strip().decode('utf-8')
        print(out)
        if "0 :" in out:
            self.use_picam2 = True
            self.picam_ids.append(0)
            if "1 :" in out:
                self.picam_ids.append(1)
            print('use_picam2')
        else:
            print('use_opencv')
        self.cap = None
        self.picam = None
        self.rate = rate
        self.msg = CompressedImage()
        self.msg.format = 'jpeg'
        print(self.msg)
        print(self.use_picam2)
        self._reader = None  # threading.Thread(target=self.continuous_capture)
        self.last_image = None
        if shm is not None:
            self.shm = shm
            self.frame_buffer = np.ndarray(
                (480, 640, 3), dtype=np.uint8, buffer=self.shm.buf)
        self.callback = callback
        self.rectify = False
        self.flip_code = 2
        self.active = False
        # self.rector = ImageRector()
        self.cameraModel = PinholeCameraModel()
        self.loadCaliFile()

    def open_camera(self, camera_id=0):
        # self.rector = ImageRector(cali_file="default.yaml")
        # self.rector = ImageRector(size=(480,360),cali_file="default.yaml")
        self.camera_id = camera_id
        try:
            if (self._reader is not None) and (self._reader.isAlive()):
                print('thread is active , stop it')
                self.active = False
                self.close_camera()
                time.sleep(0.5)

            if type(camera_id) is str:
                self.cap = cv2.VideoCapture(camera_id)
            elif camera_id >= 0:
                if self.use_picam2 and camera_id in self.picam_ids:
                    self.open_camera2(camera_id)
                    return
                else:
                    self._reader = threading.Thread(target=self.continuous_capture)
                    if hasattr(self._reader, 'is_alive'):
                        self._reader.isAlive = self._reader.is_alive
                    if self.cap is not None:
                        self.cap.release()
                        self.cap = None
                        # cv2.CAP_OPENCV_MJPEG
                    cams = self.picam_ids + cameraList()
                    print(cams, camera_id)
                    if camera_id <= len(cams):
                        self.cap = cv2.VideoCapture(
                            int(cams[camera_id].replace('/dev/video', '')))
                    elif len(cams) >= 1:
                        self.cap = cv2.VideoCapture(
                            int(cams[0].replace('/dev/video', '')))
                    else:
                        self.active = False
                        return 1
                    if self.cap.isOpened():
                        self.active = True
                        print('open camera with index %s successfully' % (camera_id))
                        self._reader.start()
                        timeout = 0
                        while self.last_image is None:
                            time.sleep(0.5)
                            timeout = timeout + 1
                            if(timeout > 20):
                                self.close_camera()
                                print('close camera')
                                return
                            print('wait for first image',timeout)
                        print('first image captured')
                        return 0
                    else:
                        print('open camera with %s failed' % (camera_id))
                        self.active = False
                        return 1

        except Exception as e:
            print('open camera with %s failed' % (camera_id))
            print(e)
            self.active = False
            return 1
        finally:
            pass

    def close_camera(self):
        print('close_camera', self.active, self.cap, self.picam)
        if self.cap is not None:
            try:
                self.cap.release()
            except Exception as e:
                print(e)
            finally:
                self.cap = None
        if self.picam is not None:
            try:
                self.picam.close()
                self.picam.stop()
            except Exception as e:
                print(e)
            finally:
                self.picam = None     
        self.active = False
        self.last_image = None

    def open_camera2(self, camera_id=0):
        try:
            if (self._reader is not None) and (self._reader.isAlive()):
                print('thread is active , stop it')
                self.active = False
                time.sleep(0.5)
            self.picam = Picamera2(camera_id)
            config = self.picam.create_still_configuration(main={"size": (640, 480), 'format': 'RGB888'})
            self.picam.configure(config)
            self.picam.start()
            self._reader = threading.Thread(target=self.continuous_capture)
            if hasattr(self._reader, 'is_alive'):
                self._reader.isAlive = self._reader.is_alive
            # if self.cap is not None:
            #     try:
            #         self.cap.release()
            #     except Exception as e:
            #         print(e)
            #     finally:
            #         self.cap = None
            self.active = True
            print('try to open picam with index %s' % (camera_id))
            self._reader.start()
            # timeout = 0
            # while self.last_image is None:
            #     time.sleep(0.5)
            #     timeout = timeout + 1
            #     if(timeout > 20):
            #         self.close_camera()
            #         print('close picam')
            #         return
            #     print('wait for first image',timeout)
            # print('first image captured')
            # return 0
        except Exception as e:
            print('open picam with %s failed' % (camera_id))
            print(e)
            self.active = False
            return 1
        finally:
            pass

    def sub_remote_camera(self, host):
        try:
            if (self._reader is not None) and (self._reader.isAlive()):
                print('thread is active , stop it')
                self.active = False
                time.sleep(0.5)
            self.active = True
            if self.cap is None:
                self.cap = WebsocketCamera(host,self.on_image_msg)
            else:
                self.cap.release()
                time.sleep(0.5)
                self.cap = WebsocketCamera(host,self.on_image_msg)

        except Exception as e:
            print(e)
        finally:
            pass

    def on_image_msg(self,json_msg):

        if self.active == False:
            self.close_camera()
            return

        # print(json_msg)
        jpg = base64.b64decode(json_msg['data'])
        # print(jpg)
        frame = bgr_from_jpg(jpg)
        frame = cv2.resize(frame, (640, 480))
        self.last_image = frame
        if self.shm is not None:
            # self.shm.lock()
            if abs(self.flip_code) <= 1:
                frame = cv2.flip(frame, self.flip_code)
            self.frame_buffer[:] = frame[:]
            # self.shm.unlock()
        if self.callback is not None:
            self.callback(frame)

    def continuous_capture(self):
        if ((self.cap is None) or (not self.cap.isOpened())) and self.picam is None:
            print('camera not opened')
            return
        if self.picam is not None:
            print('start continuous_capture picam thread')
            while self.active == True:
                try:
                    frame = self.picam.capture_array()
                    if self.shm is not None:
                        # self.shm.lock()
                        if abs(self.flip_code) <= 1:
                            frame = cv2.flip(frame, self.flip_code)
                        self.frame_buffer[:] = frame[:]
                        # self.shm.unlock()
                    self.last_image = frame
                    if self.callback is not None:
                        self.callback(frame)
                except Exception as e:
                    # raise e
                    print('picam capture error', e)
                    # time.sleep(1)
                    self.close_camera()
                finally:
                    # time.sleep(1.0/self.rate)
                    # time.sleep(0.005)
                    pass
            print('end continuous_capture picam thread')
            self.close_camera()
            return
        # self.cap.set(cv2.CAP_PROP_FPS, 30)
        # self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
        # self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
        print('start continuous_capture thread')
        while self.active == True and self.cap is not None:
            try:
                ret, frame = self.cap.read()
                # print(ret, frame is None)
                if ret == True and frame is not None:
                    frame = cv2.resize(frame, (640, 480))
                    if self.shm is not None:
                        # self.shm.lock()
                        if abs(self.flip_code) <= 1:
                            frame = cv2.flip(frame, self.flip_code)
                        self.frame_buffer[:] = frame[:]
                        # self.shm.unlock()
                    self.last_image = frame
                    if self.callback is not None:
                        self.callback(frame)
                else:
                    print('ret not success')
                    break
                    # self.last_image = None
            except Exception as e:
                # raise e
                print('camera capture error', e)
                # time.sleep(1)
                self.close_camera()
                # self.open_camera2(self.camera_id)
                return
            finally:
                # time.sleep(1.0/self.rate)
                # time.sleep(0.005)
                pass
        self.close_camera()

    def save_a_frame(self, full_path):
        if self.last_image is not None:
            try:
                cv2.imwrite(full_path, self.last_image)
                print('saved image to '+full_path)
                return 0
            except Exception as e:
                print(e)
                return 1
        else:
            return 2

    def getImage(self):
        if self.last_image is None:
            return None
        cv_image = self.last_image
        if self.camera_info_msg.width == 480:
            cv_image = cv2.resize(cv_image, (480, 360))
        if self.rectify:
            self.cameraModel.remapImage(cv_image, cv_image)
            # self.cameraModel.rectifyImage(cv_image,cv_image)
            # cv_image = self.rector.rect(cv_image)
        if self.camera_info_msg.width == 640:
            cv_image = cv2.resize(cv_image, (480, 360))

        return cv_image

    def setFlip(self, flip_code=2):
        self.flip_code = flip_code

    def setRectify(self, rectify=False):
        self.rectify = rectify

    def loadCaliFile(self, cali_file="default.yaml"):
        self.camera_info_msg = load_camera_info_3(cali_file)
        self.cameraModel.fromCameraInfo(self.camera_info_msg)


def show_pic(frame):
    cv2.imshow("capture", frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        print('q pressed')


if __name__ == '__main__':
    cam = UsbCamera(callback=show_pic)
    cam.open_camera()
    time.sleep(20)
    cam.active = False
    time.sleep(3)

    cam.open_camera()
    # cam.open_camera(1)
    time.sleep(10)
    cam.active = False
    time.sleep(3)
